//
// Created by zhuruyi on 2021/3/12.
//

#include "bsm_test.h"
#include "MessageFrame.h"

MessageFrame *test_bsm() {
    BasicSafetyMessage_t bsm_t;
    bsm_t.msgCnt = 2;

    //onstruct id
    OCTET_STRING_t id;
    //construct ctx
    asn_struct_ctx_t ctx;
    ctx.context = 0;
    ctx.ptr = nullptr;
    ctx.left = 0;
    ctx.phase = 0;
    ctx.step = 0;

    id._asn_ctx = ctx;
    //construct buff
    id.size = 8;
    //buffer size must be  8
    char vehicle_id[] = "12345678";
    id.buf = (uint8_t *) vehicle_id;

    bsm_t.id = id;

    //DSecond_t 0～59999
    DSecond_t secMark = 50000;
    bsm_t.secMark = secMark;

    //34.744998,113.654938
    Position3D_t pos;
    pos._asn_ctx = ctx;
    //ongitude jingdu
    pos.Long = 113.654938;
    pos.lat = 34.744998;
    // 海拔 分辨率0.1m   -40
    Elevation_t elevation = 200.1;
    pos.elevation = &elevation;
    bsm_t.pos = pos;

    //PositionConfidenceSet_t
    //定义位置（经纬度和高程）的综合精度
    PositionConfidenceSet_t accuracy;
    //描述了95%置信水平的车辆位置精度
    PositionConfidence_t position_confidence = PositionConfidence_a500m;
    //描述了95%置信水平的车辆度精度
    ElevationConfidence_t elevation_confidence = ElevationConfidence_elev_500_00;
    accuracy.elevation = &elevation_confidence;
    accuracy.pos = position_confidence;
    accuracy._asn_ctx = ctx;
    bsm_t.accuracy = accuracy;

    //定义车辆档位状态
    bsm_t.transmission = TransmissionState_neutral;//档
    //车速大小  分辨率为0.02m/s  数值8191表示无效数值
    bsm_t.speed = 120;
    //为车辆航向角,即为车头方向与正北方向的顺时针夹角。分辨率为 0.0125°
    bsm_t.heading = 30;
    //方向盘转角 向右侧为正 	向左为负  分辨率1.5度 数值127无效
    SteeringWheelAngle_t angle = -5;
    bsm_t.angle = &angle;

    //描述车辆运行状态的精度 。包括车速精度/ 航向精度和方向盘转角精度
    MotionConfidenceSet_t motion_confidence_set;
    //描述了95%置信水平的速精度
    SpeedConfidence_t speed = SpeedConfidence_prec100ms;
    motion_confidence_set.speedCfd = &speed;
    //描述了95%置信水平的车辆航向精度
    HeadingConfidence_t heading = HeadingConfidence_prec10deg;
    motion_confidence_set.headingCfd = &heading;
    //描述了95%置信水平的方向盘转角精度
    SteeringWheelAngleConfidence_t sw_angle = SteeringWheelAngleConfidence_prec2deg;
    motion_confidence_set.steerCfd = &sw_angle;
    motion_confidence_set._asn_ctx = ctx;
    bsm_t.motionCfd = &motion_confidence_set;

    //定义车辆四轴加速度。
    // Long 纵向加速度  向前加速为正，反向为负
    // Lat 横向加速度 向右加速为正  反向为负
    // Vert 垂直加速度  沿重力方向向下为正，反向为负
    // Yaw 横摆角速度 顺时针旋转为正，反向为负
    AccelerationSet4Way_t accel_set;
    accel_set._asn_ctx = ctx;
    //Acceleration 定义车辆加速度，分辨率为0.01m/s2 。 数值2001为无效数值
    accel_set.Long = 5;
    accel_set.lat = 3;
    accel_set.vert = 6;
    accel_set.yaw = 10;
    bsm_t.accelSet = accel_set;

    //定义系统的刹车系统状态。共有7种类型的状态：
    BrakeSystemStatus_t brakes;
    brakes._asn_ctx = ctx;
    BrakePedalStatus_t status = BrakePedalStatus_off;
    brakes.brakePadel = &status;
    brakes.wheelBrakes = nullptr;
    brakes.traction = nullptr;
    brakes.abs = nullptr;
    brakes.scs = nullptr;
    brakes.brakeBoost = nullptr;
    brakes.auxBrakes = nullptr;
    bsm_t.brakes = brakes;

    //定义车辆尺寸。由车辆长/宽/高度来定义。高度数值为可选项。
    VehicleSize_t size;
    size._asn_ctx = ctx;
    VehicleWidth_t width = 3;
    size.width = width;
    VehicleLength_t length = 5;
    size.length = length;
    VehicleHeight_t height = 3;
    size.height = &height;
    bsm_t.size = size;

    // 定义车辆的分类，可从各个维度对车辆类型进行定义。目前仅有车辆基本类型一项。
    VehicleClassification_t vehicleClass;
    //基本乘用车类型
    const int BASIC_PASSENGER_VEHICLE_TYPE = 10;
    BasicVehicleClass_t basic_vehicle_class = BASIC_PASSENGER_VEHICLE_TYPE;
    vehicleClass.classification = basic_vehicle_class;
    vehicleClass._asn_ctx = ctx;
    bsm_t.vehicleClass = vehicleClass;

    VehicleSafetyExtensions_t safety_extensions;
    //定义车辆安全辅助信息集合。用于BSM消息中，作为基础安全数据的补充。包括车辆特殊事件状态/车辆历史轨迹/路线预测/车身灯光状态等。各项辅助信息均为可选项。
    VehicleEventFlags_t eventFlags;
    eventFlags.size = 2;
    eventFlags.bits_unused = 3;
    short event_buf = 0x0001;
    eventFlags.buf = (uint8_t *) &event_buf;
    safety_extensions.events = &eventFlags;
    //定义车辆历史轨迹。
    safety_extensions.pathHistory = nullptr;
    //定义车辆的预测路线 主要是预测线路的	曲率半径
    safety_extensions.pathPrediction = nullptr;
    //定义车身周围的车灯状态
    safety_extensions.lights = nullptr;
    safety_extensions._asn_ctx = ctx;
    bsm_t.safetyExt = &safety_extensions;

    bsm_t._asn_ctx = ctx;

    //construct MessageFrame
    MessageFrame_t message_frame;
    MessageFrame_PR message_frame_pr = MessageFrame_PR_bsmFrame;
    message_frame.present = message_frame_pr;
    message_frame.choice.bsmFrame = bsm_t;


//    xer_encode(0, &bsm_t, XER_F_BASIC, )
    xer_fprint(stdout, &asn_DEF_MessageFrame, &message_frame);
    return &message_frame;
}